﻿#include "data_fushion.h"


CalibPclImg::CalibPclImg()
{
	subPointCloud = nh.subscribe<sensor_msgs::PointCloud2>("rslidar_points", 1, &CalibPclImg::pcl2img, this);
	subObj = nh.subscribe("bbox_msg", 1, &CalibPclImg::ObjCallback, this);
	subGPS = nh.subscribe<token_msgs::Nav_ori>("nav_msg", 1, &CalibPclImg::NavCallback, this);

	/// 加载标定参数
	transfromEigentoCamera<< 2.4218716644699434e-03, -9.9999449089368175e-01, -2.2699603360241682e-03,  5.4486728759709918e-02,
							-8.5947329009445905e-03,  2.2490678031144018e-03, -9.9996053535146001e-01, -2.0519654677992365e-01,
							 9.9996013175726284e-01,  2.4412857889399286e-03, -8.5892385980634867e-03, -2.0146010826086281e-01,
                                                  0,                       0,                       0,                       1;

	cameraPara<< 1.8666088127172050e+03,                      0, 6.1897171513522983e+02,                      0,
									  0, 1.9552563369016768e+03, 5.1613593890827763e+02,                      0,
									  0,                      0,                      1,                      0,
									  0,                      0,                      0,                      1;
}

void CalibPclImg::pcl2img(const sensor_msgs::PointCloud2ConstPtr& ptmsg)
{
	pcl::PointCloud<PointType>::Ptr pcl_cloud(new pcl::PointCloud<PointType>);
    pcl::fromROSMsg(*ptmsg, *pcl_cloud);
	pcl_cloud_ =* pcl_cloud;
}

void CalibPclImg::ObjCallback(const std_msgs::Float64MultiArray::ConstPtr& bboxmsg)
{
	pcl::PointCloud<PointType>::Ptr targetPts(new pcl::PointCloud<PointType>);

	for(size_t i = 0; i < pcl_cloud_.size(); i++)
	{
		if (pcl_cloud_.points[i].x > 0)
		{
			thispoint << pcl_cloud_.points[i].x, pcl_cloud_.points[i].y, pcl_cloud_.points[i].z, 1;

			thispoint = cameraPara*(transfromEigentoCamera*thispoint);
			thispoint /= thispoint[2];
			
			if (bboxmsg->data[0]*img_w < thispoint[0] && thispoint[0] < bboxmsg->data[0]*img_w + bboxmsg->data[2]*img_w
				&& bboxmsg->data[1]*img_w < thispoint[1] && thispoint[1] < bboxmsg->data[1]*img_w + bboxmsg->data[3]*img_w)
			{
				targetPts->points.push_back(pcl_cloud_.points[i]);
			}
		}
	}

	std::vector<pcl::PointIndices>ece_inlier;
	pcl::search::KdTree<PointType>::Ptr tree(new pcl::search::KdTree<PointType>);
	pcl::EuclideanClusterExtraction<PointType> ece;
	ece.setInputCloud(targetPts);
	ece.setClusterTolerance(0.5);
	ece.setMinClusterSize(10);
	ece.setMaxClusterSize(20000);
	ece.setSearchMethod(tree);
	ece.extract(ece_inlier);

	if (ece_inlier.size() < 1)
	{
		ROS_INFO("Cluster extract failed!");
		return;
	}
	
	int max_num = 0;
	std::vector<pcl::PointCloud<PointType>::Ptr> obj_points;
	for (size_t i = 0; i < ece_inlier.size(); i++)
	{
		pcl::PointCloud<PointType>::Ptr tmp_ob(new pcl::PointCloud<PointType>);
		pcl::ExtractIndices<PointType> extract_ground;
		extract_ground.setInputCloud(targetPts);
		extract_ground.setIndices(boost::make_shared<pcl::PointIndices>(ece_inlier[i]));

		extract_ground.setNegative(false); //true removes the indices, false leaves only the indices
		extract_ground.filter(*tmp_ob);
		obj_points.push_back(tmp_ob);
		if (tmp_ob->size() > obj_points[max_num]->size())
		{
			max_num = i;
		}
	}

	for (size_t i = 0; i < obj_points[max_num]->size(); i++)
	{
		centerPt.x += obj_points[max_num]->points[i].x;
		centerPt.y += obj_points[max_num]->points[i].y;
	}
	centerPt.x /= obj_points[max_num]->size();
	centerPt.y /= obj_points[max_num]->size();
	centerPt.z = 0;

	float angle_tmp = atan(centerPt.x / centerPt.y) * 180 / M_PI;
	angle_tmp += nav_info.heading;
	if (angle_tmp > 360){angle_tmp -= 360;}
	else if (angle_tmp < 0){angle_tmp += 360;}

	float obj_dis = sqrt(pow(centerPt.x, 2)+pow(centerPt.y, 2));
	float global_x, global_y;
	global_x = sin(angle_tmp * M_PI / 180) * obj_dis;
	global_y = cos(angle_tmp * M_PI / 180) * obj_dis;
	
	float obj_latitude = nav_info.latitude + global_y * m2l;
	float obj_longitude = nav_info.longitude + global_x * (m2l/cos(nav_info.latitude*M_PI/180));

	ROS_INFO("distance:%f, x:%f, y:%f", obj_dis, centerPt.x, centerPt.y);
	ROS_INFO("%f, %f",obj_latitude, obj_longitude);
	centerPt.x = 0;
	centerPt.y = 0;
}

void CalibPclImg::NavCallback(const token_msgs::Nav_ori::ConstPtr& navmsg){
	nav_info =* navmsg;
	// ROS_INFO("GPS info received");
}